import threading
import rclpy
from rclpy.node import Node
import requests
import time
import json
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import Twist
from std_msgs.msg import String, Bool
import datetime
from qiniu import Auth, put_file, etag
import qiniu.config
import os
import threading

class TelemetrySender(Node):
    def __init__(self):
        super().__init__('telemetry_sender')
        
        # 声明节点参数
        self.declare_parameter('drone_id', 1)
        self.declare_parameter('drone_service_url', 'http://124.70.160.214:8080')
        
        # 获取参数值
        self.drone_id = self.get_parameter('drone_id').value
        self.put_telemetry_url = self.get_parameter('drone_service_url').value+'/monitor/putTelemetry'
        self.get_speed_control_url = self.get_parameter('drone_service_url').value+'/monitor/getFlyControlData'
        self.get_order_control_url = self.get_parameter('drone_service_url').value+'/monitor/getDroneOrder'
        self.get_all_active_routes_url = self.get_parameter('drone_service_url').value+'/routes/getAllActiveRoutes'
        self.get_all_actions_url = self.get_parameter('drone_service_url').value+'/actions/getAllDroneAction'
        self.add_fire_url = self.get_parameter('drone_service_url').value+'/fires/add'
        
        # 初始化传感器数据存储
        self.current_gps = NavSatFix()
        self.current_attitude = String()
        self.current_velocity = Twist()
        self.current_speed = 0.0
        self.system_usage = 0.0
        self.lastSpeedCommandTimestamp=0.0
        self.mode_state=""

        self.old_control_commands=[]
        self.active_routes_datas=[]
        self.route_id=-99
        self.actions=[]
        self.video_path=""
        self.is_updating_video=False
        self.images_url=''


        # 创建话题订阅
        self.create_subscription(NavSatFix, '/apm_drone/current_GPS', self.gps_callback, 10)
        self.create_subscription(String, '/apm_drone/current_attitude', self.attitude_callback, 10)
        self.create_subscription(Twist, '/apm_drone/current_velocity', self.velocity_callback, 10)
        self.create_subscription(String, '/apm_drone/current_mode_state', self.mode_state_callback, 10)
        self.create_subscription(String, '/system_info', self.system_info_callback, 10)
        self.create_subscription(String, '/video_path', self.video_path_callback, 10)

        # 创建控制指令发布器
        self.publisher_bt_enable = self.create_publisher(Bool, '/apm_drone/bt_enable', 10)
        self.publisher_target_gps = self.create_publisher(NavSatFix, '/apm_drone/target_gps_location', 10)
        self.publisher_mult_target_gps = self.create_publisher(String, '/apm_drone/mult_target_gps', 10)
        self.publisher_target_mode = self.create_publisher(String, '/apm_drone/target_mode', 10)
        self.publisher_high_permission_vel = self.create_publisher(Twist, '/apm_drone/high_permission_target_cmd_vel', 10)

        # 创建定时器
        self.process_network_data_timer_4hz = self.create_timer(0.25, self.process_network_data_4hz)
        self.process_network_data_timer_1hz = self.create_timer(1.0, self.process_network_data_1hz)
        self.process_network_data_timer_10s = self.create_timer(10.0, self.process_network_data_10s)
        self.process_network_data_timer_60s = self.create_timer(60.0, self.process_network_data_60s)

        self.get_logger().info(f"无人机遥测节点启动，ID: {self.drone_id}")


    def video_path_callback(self, msg):
        if (not self.is_updating_video) and (self.video_path != msg.data):
            self.is_updating_video = True
            self.video_path = msg.data
            threading.Thread(target=self.update_video, daemon=True).start()

    def update_video(self):
        try:
            # 验证路径有效性
            if not os.path.exists(self.video_path):
                print(f"路径不存在: {self.video_path}")
                return
                
            if not os.path.isdir(self.video_path):
                print(f"这不是一个有效文件夹: {self.video_path}")
                return

            print(f"\n正在扫描文件夹: {self.video_path}")
            
            # 使用os.scandir高效遍历目录
            with os.scandir(self.video_path) as entries:
                # 过滤出文件并排序（排除目录）
                files = sorted([entry.name for entry in entries if entry.is_file()])
                
                if not files:
                    print("文件夹中没有文件")
                    return
                    
                print(f"找到 {len(files)} 个文件：")
                self.images_url=''
                for index, filename in enumerate(files, 1):
                    print(f"{index}. {filename}")
                    if self.upload_to_qiniuCloud(filename)[0]:
                        if self.images_url=='':
                            self.images_url=self.upload_to_qiniuCloud(filename)[1]
                        else:
                            self.images_url=self.images_url+','+self.upload_to_qiniuCloud(filename)[1]
                if self.images_url!='':
                    self.report_fire_incident(self.images_url)
        except PermissionError:
            print("权限不足，无法访问文件夹")
        except Exception as e:
            print(f"扫描文件时发生错误: {str(e)}")
        self.is_updating_video = False
        

    def upload_to_qiniuCloud(self,fliename):
        # 需要填写你的 Access Key 和 Secret Key
        access_key = '1rjPikwdnDDTddwZ8Bzm5YMt8V-0Id-__OXqCKgt'
        secret_key = '5ChtwHdHIJxfCff_JNeUVPTp-bvLXTkhnnLw9o4S'
        # 空间名称
        bucket_name = '250417drone'
        # 域名
        domain_name='http://suujj5yg5.hd-bkt.clouddn.com/'
        # 上传文件的本地路径
        local_file_path =self.video_path+"/"+fliename
        # 上传到七牛后保存的文件名（如果为None，会使用文件内容的hash作为文件名）
        qiniu_file_name = fliename

        # 构建鉴权对象
        q = Auth(access_key, secret_key)

        # 生成上传 Token，可以指定过期时间等
        token = q.upload_token(bucket_name, qiniu_file_name, 3600)

        # 上传文件
        ret, info = put_file(token, qiniu_file_name, local_file_path)

        print(ret)  # 上传返回的结果
        print(info)  # 上传返回的详细信息

        if ret is not None:
            print('Upload success!')
            # 如果配置了域名，可以通过以下方式访问（需要替换域名）
            print('File URL: http://{0}/{1}'.format('your_domain', ret['key']))
            return [True,domain_name+fliename]
        else:
            print('Upload failed')
            return [False]

        # # 验证上传是否成功
        # assert ret['key'] == qiniu_file_name
        # assert ret['hash'] == etag(local_file_path)

    def report_fire_incident(self, video_url):
        """上报火情到服务器"""
        try:
            # 获取当前GPS数据
            current_lat = float(self.current_gps.latitude)
            current_lon = float(self.current_gps.longitude)
            # 构造请求参数
            params = {
                "videoPath": video_url,
                "latitude": current_lat,
                "longitude": current_lon
            }

            # 发送PUT请求
            response = requests.put(self.add_fire_url, params=params)
            response.raise_for_status()
            
            # 处理响应
            if response.status_code == 200:
                resp_data = response.json()
                if resp_data.get("code") == 200:
                    self.get_logger().info("火情上报成功")
                    return True
                else:
                    self.get_logger().error(f"火情上报失败: {resp_data.get('message', '未知错误')}")
            return False

        except requests.exceptions.RequestException as e:
            self.get_logger().error(f"火情上报网络错误: {str(e)}")
        except Exception as e:
            self.get_logger().error(f"火情上报异常: {str(e)}")
        return False


    def mode_state_callback(self,msg):
        self.mode_state=msg.data

    def system_info_callback(self, msg):
        """系统信息回调处理"""
        try:
            system_data = json.loads(msg.data)
            # 使用CPU平均使用率（保留两位小数）
            self.system_usage = round(system_data.get("cpu_avg", 0.0), 2)
            self.get_logger().debug(f"更新系统使用率: {self.system_usage}%")
        except (json.JSONDecodeError, KeyError) as e:
            self.get_logger().error(f"系统信息解析失败: {str(e)}")
            self.system_usage = 0.0  # 重置为默认值

    def gps_callback(self, msg):
        """GPS数据回调"""
        self.current_gps = msg
        self.get_logger().debug(f"更新GPS数据: {msg.latitude}, {msg.longitude}")

    def attitude_callback(self, msg):
        """姿态数据回调"""
        self.current_attitude = msg
        self.get_logger().debug(f"更新姿态数据: {msg.data}")

    def velocity_callback(self, msg):
        """速度数据回调"""
        self.current_velocity = msg
        self.current_speed = (msg.linear.x**2 + msg.linear.y**2 + msg.linear.z**2)**0.5
        self.get_logger().debug(f"更新速度数据: {self.current_speed:.2f} m/s")

    def process_network_data_4hz(self):
        """遥测数据发送回调"""
        telemetry_data = {
            "current_gps": {
                "latitude": self.current_gps.latitude,
                "longitude": self.current_gps.longitude
            },
            "current_speed": round(self.current_speed, 2),
            "route_id": self.route_id,
            "altitude": round(self.current_gps.altitude, 2),
            "system_usage": self.system_usage,
            "timestamp": int(time.time() * 1000)
        }
        self.send_telemetry(telemetry_data)
        """控制指令获取回调"""
        self.fetch_speed_control_command()


    def process_network_data_1hz(self):
        self.fetch_order_control_command()

    def process_network_data_10s(self):
        self.fetch_all_active_routes()

    def process_network_data_60s(self):
        self.fetch_all_actions()

    def fetch_all_actions(self):
        try:
            response = requests.get(self.get_all_actions_url)
            responseJson = json.loads(response.text)
            if responseJson["code"] == 200:
                self.actions=responseJson.get("data", {})
                current_time = datetime.datetime.now()
                current_time = current_time.replace(microsecond=0)
                # current_hms = (current_time.hour, current_time.minute, current_time.second)

                # 正确调用方式示例
                self.get_logger().info(
                    f"Fetched action: {self.actions}",  # 消息内容
                    throttle_duration_sec=1.0  # 可选：节流参数
                )
                for action in self.actions:
                    if action["droneId"]==self.drone_id and action["enabled"]:
                        start_time_str = action["startTime"].split('+')[0].rstrip('Z')  # 去除时区部分
                        target_time = datetime.datetime.fromisoformat(start_time_str).time()
                        if target_time.hour != current_time.hour or target_time.minute != current_time.minute:
                            continue
                        schedule_type = action["scheduleType"]
                        if schedule_type == "daily" or schedule_type == "once":
                            threading.Thread(target=self.bt_start, args=(action["routeId"],), daemon=True).start()
                        elif schedule_type == "weekly" and current_time.isoweekday() == action["whichDay"]:  # ISO weekday标准（1=周一，7=周日）
                            threading.Thread(target=self.bt_start, args=(action["routeId"],), daemon=True).start()
                        elif schedule_type == "monthly" and current_time.day == action["whichDay"]:
                            threading.Thread(target=self.bt_start, args=(action["routeId"],), daemon=True).start()
                    
            else:
                self.get_logger().warning(f"获取动作失败 | 状态码: {response.status_code}")
        except Exception as e:
            self.get_logger().error(f"获取动作时出现异常: {str(e)}")


    def bt_start(self,route_id):
        print(route_id)
        print(self.active_routes_datas)
        if(self.mode_state=="armed"):
            return
        
        duration = 2.0
        rate = 2
        interval = 1.0 / rate
        end_time = time.time() + duration
        msg = String()
        msg.data="null"
        for route in self.active_routes_datas:
            if route["routeId"]==route_id:
                msg.data = route["coordinates"]
        if msg.data=="null":
            return

        self.get_logger().info('发送gps数据')
        self.route_id=route_id
        while time.time() < end_time and rclpy.ok():
            print(msg.data)
            self.publisher_mult_target_gps.publish(msg)
            time.sleep(interval)

        self.get_logger().info('启动行为树')
        duration = 2.0
        rate = 10
        interval = 1.0 / rate
        end_time = time.time() + duration
        msg = Bool()
        msg.data = True
        while time.time() < end_time and rclpy.ok():
            self.publisher_bt_enable.publish(msg)
            time.sleep(interval)
        
        # while rclpy.ok():
        #     if(self.mode_state=="disarmed"):
        #         self.route_id=-99
        #         self.land()
        #         return
        #     time.sleep(2.0)


    def fetch_all_active_routes(self):
        """获取有效路线"""
        try:
            response = requests.get(self.get_all_active_routes_url)
            responseJson = json.loads(response.text)
            if responseJson["code"] == 200:
                self.active_routes_datas=responseJson.get("data", {})
                self.get_logger().info("更新路线")
            else:
                self.get_logger().warning(f"获取有效路线失败 | 状态码: {response.status_code}")
        except Exception as e:
            self.get_logger().error(f"获取有效路线异常: {str(e)}")

    def fetch_order_control_command(self):
        """获取并处理飞行控制指令"""
        params = {"drone_id": self.drone_id}
        try:
            response = requests.get(self.get_order_control_url, params=params)
            responseJson = json.loads(response.text)
            if responseJson["code"] == 200:
                control_datas = responseJson.get("data", {})
                if not control_datas:
                    return
                if((time.time() * 1000-control_datas[-1])>5000):
                    return
                if(len(self.old_control_commands)==len(control_datas)):
                    i=0
                    s=0
                    while(i<len(control_datas)):
                        if(self.old_control_commands[i]==control_datas[i]):
                            s=s+1
                        i=i+1
                    if(s==len(control_datas)):
                        return
                # 添加命令类型判断和线程启动
                if control_datas[0] == "land":
                    self.get_logger().info("接收到降落指令")
                    threading.Thread(target=self.land, daemon=True).start()
                    
                elif control_datas[0] == "take_off":
                    if len(control_datas) == 3:
                        self.get_logger().info("接收到有路线起飞指令")
                        threading.Thread(target=self.bt_start, args=(control_datas[1],), daemon=True).start()
                    else:
                        self.get_logger().info("接收到无路线起飞指令")
                        threading.Thread(target=self.takeoff_without_route, daemon=True).start()
                self.old_control_commands=control_datas
            else:
                self.get_logger().warning(f"命令指令获取失败 | 状态码: {response.status_code}")
                

        except Exception as e:
            self.get_logger().error(f"命令控制指令处理异常: {str(e)}")

    def land(self):
        """降落指令执行函数"""
        try:
            # 第一阶段：发送bt_enable=false，10Hz持续1秒
            self.get_logger().info("开始发送降落指令第一阶段：禁用导航")
            duration = 2.0
            rate = 10
            interval = 1.0 / rate
            end_time = time.time() + duration
            
            while time.time() < end_time and rclpy.ok():
                msg = Bool()
                msg.data = False
                self.publisher_bt_enable.publish(msg)
                time.sleep(interval)

            # 第二阶段：发送LAND模式指令，5Hz持续4秒
            self.get_logger().info("开始发送降落指令第二阶段：切换至LAND模式")
            self._send_mode_commands("LAND", duration=4, rate=5)
            self.get_logger().info("降落指令执行完成")
        except Exception as e:
            self.get_logger().error(f"降落过程中发生异常: {str(e)}")

    def takeoff_without_route(self):
        """无路线起飞指令执行函数"""
        try:
            # 发送GUIDED模式指令
            self._send_mode_commands("GUIDED", duration=2, rate=5)
            # 发送ARM指令
            self._send_mode_commands("ARM", duration=2, rate=5)
            # 发送TAKEOFF指令
            self._send_mode_commands("TAKEOFF", duration=10, rate=5)
            self._send_mode_commands("GUIDED", duration=2, rate=5)
            self.get_logger().info("无路线起飞指令执行完成")
        except Exception as e:
            self.get_logger().error(f"起飞过程中发生异常: {str(e)}")

    def _send_mode_commands(self, mode, duration, rate):
        """通用模式指令发送函数"""
        interval = 1.0 / rate
        end_time = time.time() + duration
        self.get_logger().info(f"开始发送{mode}模式指令")
        
        while time.time() < end_time and rclpy.ok():
            msg = String()
            msg.data = mode
            self.publisher_target_mode.publish(msg)
            time.sleep(interval)


    def fetch_speed_control_command(self):
        """获取并处理飞行控制指令"""
        params = {"drone_id": self.drone_id}
        try:
            response = requests.get(self.get_speed_control_url, params=params)
            responseJson = json.loads(response.text)
            if responseJson["code"] == 200:
                control_data = responseJson.get("data", {}) 
                if not control_data:
                    # self.get_logger().warning("控制指令数据为空，跳过处理")
                    return
                if((control_data["timestamp"]-self.lastSpeedCommandTimestamp)>200):
                    self.lastSpeedCommandTimestamp=control_data["timestamp"]

                    vel_msg = Twist()
                    
                    # 设置线速度（单位：米/秒）
                    vel_msg.linear.x = float(control_data["xSpeed"])/1000
                    vel_msg.linear.y = float(control_data["ySpeed"])/1000
                    vel_msg.linear.z = 0.0
                    
                    # 设置角速度（单位：弧度/秒）
                    vel_msg.angular.z = float(control_data["rotationSpeed"])/1000
                    self.publisher_high_permission_vel.publish(vel_msg)
                    self.get_logger().info(
                        f"发布速度指令: X:{vel_msg.linear.x:.2f} "
                        f"Y:{vel_msg.linear.y:.2f} "
                        f"Z:{vel_msg.linear.z:.2f} "
                        f"Yaw:{vel_msg.angular.z:.2f}"
                    )
            else:
                self.get_logger().warning(f"速度控制指令获取失败 | 状态码: {response.status_code}")
                

        except Exception as e:
            self.get_logger().error(f"速度控制指令处理异常: {str(e)}")

    def send_telemetry(self, data):
        """发送遥测数据"""
        params = {"drone_id": self.drone_id}
        try:
            response = requests.put(self.put_telemetry_url, params=params, json=data)
            if response.status_code != 200:
                self.get_logger().error(f"遥测发送失败: {response.status_code}")
        except Exception as e:
            self.get_logger().error(f"遥测发送异常: {str(e)}")

def main(args=None):
    rclpy.init(args=args)
    node = TelemetrySender()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("节点终止")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

